﻿#include "ss3dMathFunction.h"

namespace StarSeeker
{
namespace Math3D
{

//快速计算平方根倒数，1/√(x)，近似值不是精确值
float FastRsqrt(float num)
{
	int i;
	float x2, y;
	const float threeHalfs = 1.5f;

	x2 = num * 0.5f;
	y = num;
	i = *(int*)(&y);
	i = 0x5f3759df - (i >> 1);
	y = *(float*)(&i);
	y = y * (threeHalfs - (x2*y*y));
	//y = y * (threeHalfs - (x2*y*y));//2次牛顿迭代法后，结果和精确值就很接近了
	return y;
}
//两点距离
ssfloat TwoPositionDistance3(ssfloat fP1x, ssfloat fP1y, ssfloat fP1z, ssfloat fP2x, ssfloat fP2y, ssfloat fP2z)
{
	return Sqrt((fP1x - fP2x)*(fP1x - fP2x) + (fP1y - fP2y)*(fP1y - fP2y) + (fP1z - fP2z)*(fP1z - fP2z));
};
ssfloat TwoPositionDistance2(ssfloat fP1x, ssfloat fP1y, ssfloat fP2x, ssfloat fP2y)
{
	return Sqrt((fP1x - fP2x)*(fP1x - fP2x) + (fP1y - fP2y)*(fP1y - fP2y));
};
//向量点乘
ssfloat Float3XDot(const ssFLOAT3 *pIn1, const ssFLOAT3 *pIn2)
{
	return pIn1->x*pIn2->x + pIn1->y*pIn2->y + pIn1->z*pIn2->z;
}
//向量夹角
ssfloat Float3XIncludedAngle(const ssFLOAT3X * pIn1, const ssFLOAT3X * pIn2)
{
	ssfloat angle = Float3XDot(pIn1, pIn2);
	if (angle < -1.0f)
		angle = -1.0f;
	else if (angle > 1.0f)
		angle = 1.0f;

	return ACos(angle);
}
ssfloat Float3XNormalizeIncludedAngle(const ssFLOAT3X * pIn1, const ssFLOAT3X * pIn2)
{
	ssFLOAT3X a = pIn1->Normalized();
	ssFLOAT3X b = pIn2->Normalized();
	return Float3XIncludedAngle(&a, &b);
}
StarSeeker::ssfloat Float2XIncludedAngle(const ssFLOAT2X *pIn1, const ssFLOAT2X *pIn2)
{
	ssfloat angle = pIn1->x*pIn2->x + pIn1->y*pIn2->y;
	if (angle < -1.0f)
		angle = -1.0f;
	else if (angle > 1.0f)
		angle = 1.0f;

	return ACos(angle);
}
StarSeeker::ssfloat Float2XNormalizeIncludedAngle(const ssFLOAT2X *pIn1, const ssFLOAT2X *pIn2)
{
	ssFLOAT2X a = pIn1->Normalized();
	ssFLOAT2X b = pIn2->Normalized();
	return Float2XIncludedAngle(&a, &b);
}
//向量叉乘
void Float3XCross(ssFLOAT3 *pOut, const ssFLOAT3 *pIn1, const ssFLOAT3 *pIn2)
{
	pOut->x = pIn1->y*pIn2->z - pIn2->y*pIn1->z;
	pOut->y = pIn1->z*pIn2->x - pIn1->x*pIn2->z;
	pOut->z = pIn1->x*pIn2->y - pIn1->y*pIn2->x;
}
void Float3XNormalCross(ssFLOAT3X *pOut, ssFLOAT3X *pIn1, ssFLOAT3X *pIn2)
{
	ssFLOAT3X a = pIn1->Normalized();
	ssFLOAT3X b = pIn2->Normalized();
	pOut->x = a.y*b.z - b.y*a.z;
	pOut->y = a.z*b.x - a.x*b.z;
	pOut->z = a.x*b.y - a.y*b.x;
}
//矩阵乘法
void FLOAT4x4XMultiply(ssFLOAT4x4 *pOut, const ssFLOAT4x4 *pIn1, const ssFLOAT4x4 *pIn2)
{
	pOut->_11 = (pIn1->_11*pIn2->_11) + (pIn1->_12*pIn2->_21) + (pIn1->_13*pIn2->_31) + (pIn1->_14*pIn2->_41);
	pOut->_12 = (pIn1->_11*pIn2->_12) + (pIn1->_12*pIn2->_22) + (pIn1->_13*pIn2->_32) + (pIn1->_14*pIn2->_42);
	pOut->_13 = (pIn1->_11*pIn2->_13) + (pIn1->_12*pIn2->_23) + (pIn1->_13*pIn2->_33) + (pIn1->_14*pIn2->_43);
	pOut->_14 = (pIn1->_11*pIn2->_14) + (pIn1->_12*pIn2->_24) + (pIn1->_13*pIn2->_34) + (pIn1->_14*pIn2->_44);
	pOut->_21 = (pIn1->_21*pIn2->_11) + (pIn1->_22*pIn2->_21) + (pIn1->_23*pIn2->_31) + (pIn1->_24*pIn2->_41);
	pOut->_22 = (pIn1->_21*pIn2->_12) + (pIn1->_22*pIn2->_22) + (pIn1->_23*pIn2->_32) + (pIn1->_24*pIn2->_42);
	pOut->_23 = (pIn1->_21*pIn2->_13) + (pIn1->_22*pIn2->_23) + (pIn1->_23*pIn2->_33) + (pIn1->_24*pIn2->_43);
	pOut->_24 = (pIn1->_21*pIn2->_14) + (pIn1->_22*pIn2->_24) + (pIn1->_23*pIn2->_34) + (pIn1->_24*pIn2->_44);
	pOut->_31 = (pIn1->_31*pIn2->_11) + (pIn1->_32*pIn2->_21) + (pIn1->_33*pIn2->_31) + (pIn1->_34*pIn2->_41);
	pOut->_32 = (pIn1->_31*pIn2->_12) + (pIn1->_32*pIn2->_22) + (pIn1->_33*pIn2->_32) + (pIn1->_34*pIn2->_42);
	pOut->_33 = (pIn1->_31*pIn2->_13) + (pIn1->_32*pIn2->_23) + (pIn1->_33*pIn2->_33) + (pIn1->_34*pIn2->_43);
	pOut->_34 = (pIn1->_31*pIn2->_14) + (pIn1->_32*pIn2->_24) + (pIn1->_33*pIn2->_34) + (pIn1->_34*pIn2->_44);
	pOut->_41 = (pIn1->_41*pIn2->_11) + (pIn1->_42*pIn2->_21) + (pIn1->_43*pIn2->_31) + (pIn1->_44*pIn2->_41);
	pOut->_42 = (pIn1->_41*pIn2->_12) + (pIn1->_42*pIn2->_22) + (pIn1->_43*pIn2->_32) + (pIn1->_44*pIn2->_42);
	pOut->_43 = (pIn1->_41*pIn2->_13) + (pIn1->_42*pIn2->_23) + (pIn1->_43*pIn2->_33) + (pIn1->_44*pIn2->_43);
	pOut->_44 = (pIn1->_41*pIn2->_14) + (pIn1->_42*pIn2->_24) + (pIn1->_43*pIn2->_34) + (pIn1->_44*pIn2->_44);
}
//坐标乘矩阵
void FLOAT4x4XTransformCoord(ssFLOAT3X *pOut, const ssFLOAT4x4X *pIn1, const ssFLOAT3X *pIn2)
{
	ssfloat w = 1.f / (pIn2->x * pIn1->_14 + pIn2->y * pIn1->_24 + pIn2->z * pIn1->_34 + pIn1->_44);
	pOut->x = (pIn2->x * pIn1->_11 + pIn2->y * pIn1->_21 + pIn2->z * pIn1->_31 + pIn1->_41) * w;
	pOut->y = (pIn2->x * pIn1->_12 + pIn2->y * pIn1->_22 + pIn2->z * pIn1->_32 + pIn1->_42) * w;
	pOut->z = (pIn2->x * pIn1->_13 + pIn2->y * pIn1->_23 + pIn2->z * pIn1->_33 + pIn1->_43) * w;
}
//向量乘矩阵
void FLOAT4x4XTransformVector3(ssFLOAT3X *pOut, const ssFLOAT4x4X *pIn1, const ssFLOAT3X *pIn2)
{
	pOut->x = pIn2->x * pIn1->_11 + pIn2->y * pIn1->_21 + pIn2->z * pIn1->_31;
	pOut->y = pIn2->x * pIn1->_12 + pIn2->y * pIn1->_22 + pIn2->z * pIn1->_32;
	pOut->z = pIn2->x * pIn1->_13 + pIn2->y * pIn1->_23 + pIn2->z * pIn1->_33;
}
void FLOAT3x3XTransformVector3(ssFLOAT3X *pOut, const ssFLOAT3x3X *pIn1, const ssFLOAT3X *pIn2)
{
	pOut->x = pIn2->x * pIn1->_11 + pIn2->y * pIn1->_21 + pIn2->z * pIn1->_31;
	pOut->y = pIn2->x * pIn1->_12 + pIn2->y * pIn1->_22 + pIn2->z * pIn1->_32;
	pOut->z = pIn2->x * pIn1->_13 + pIn2->y * pIn1->_23 + pIn2->z * pIn1->_33;
}
void FLOAT4x4XTransformVector4(ssFLOAT4X *pOut, const ssFLOAT4x4X *pIn1, const ssFLOAT4X *pIn2)
{
	pOut->m[0] = pIn1->m[0] * pIn2->m[0] + pIn1->m[4] * pIn2->m[1] + pIn1->m[8] * pIn2->m[2] + pIn1->m[12] * pIn2->m[3];
	pOut->m[1] = pIn1->m[1] * pIn2->m[0] + pIn1->m[5] * pIn2->m[1] + pIn1->m[9] * pIn2->m[2] + pIn1->m[13] * pIn2->m[3];
	pOut->m[2] = pIn1->m[2] * pIn2->m[0] + pIn1->m[6] * pIn2->m[1] + pIn1->m[10] * pIn2->m[2] + pIn1->m[14] * pIn2->m[3];
	pOut->m[3] = pIn1->m[3] * pIn2->m[0] + pIn1->m[7] * pIn2->m[1] + pIn1->m[11] * pIn2->m[2] + pIn1->m[15] * pIn2->m[3];
}

//绕X轴旋转的矩阵
void FLOAT4x4XRotX(ssFLOAT4x4X *pIn, ssfloat angle)
{
	ssfloat fcos = Cos(angle);
	ssfloat fsin = Sin(angle);
	pIn->Identity();
	pIn->m2[1][1] = pIn->m2[2][2] = fcos;
	pIn->m2[2][1] = -fsin;
	pIn->m2[1][2] = fsin;
}
void FLOAT3x3XRotX(ssFLOAT3x3X *pIn, ssfloat angle)
{
	ssfloat fcos = Cos(angle);
	ssfloat fsin = Sin(angle);
	pIn->Identity();
	pIn->m2[1][1] = pIn->m2[2][2] = fcos;
	pIn->m2[2][1] = -fsin;
	pIn->m2[1][2] = fsin;
}
//绕Y轴旋转的矩阵
void FLOAT4x4XRotY(ssFLOAT4x4X *pOut, ssfloat angle)
{
	ssfloat fcos = Cos(angle);
	ssfloat fsin = Sin(angle);
	pOut->Identity();
	pOut->m2[0][0] = pOut->m2[2][2] = fcos;
	pOut->m2[2][0] = fsin;
	pOut->m2[0][2] = -fsin;
}
void FLOAT3x3XRotY(ssFLOAT3x3X *pOut, ssfloat angle)
{
	ssfloat fcos = Cos(angle);
	ssfloat fsin = Sin(angle);
	pOut->Identity();
	pOut->m2[0][0] = pOut->m2[2][2] = fcos;
	pOut->m2[2][0] = fsin;
	pOut->m2[0][2] = -fsin;
}
//绕Z轴旋转的矩阵
void FLOAT4x4XRotZ(ssFLOAT4x4X *pOut, ssfloat angle)
{
	ssfloat fcos = Cos(angle);
	ssfloat fsin = Sin(angle);
	pOut->Identity();
	pOut->m2[0][0] = pOut->m2[1][1] = fcos;
	pOut->m2[1][0] = -fsin;
	pOut->m2[0][1] = fsin;
}
void FLOAT3x3XRotZ(ssFLOAT3x3X *pOut, ssfloat angle)
{
	ssfloat fcos = Cos(angle);
	ssfloat fsin = Sin(angle);
	pOut->Identity();
	pOut->m2[0][0] = pOut->m2[1][1] = fcos;
	pOut->m2[1][0] = -fsin;
	pOut->m2[0][1] = fsin;
}
//绕任意轴旋转
void FLOAT4x4XRotation(ssFLOAT4x4X *pOut, ssfloat angle, ssFLOAT3X *pAxis)
{
	pAxis->Normalize();

	ssfloat fcos = Cos(angle);
	ssfloat fsin = Sin(angle);

	pOut->m2[0][0] = (pAxis->x * pAxis->x) * (1.0f - fcos) + fcos;
	pOut->m2[1][0] = (pAxis->x * pAxis->y) * (1.0f - fcos) - (pAxis->z * fsin);
	pOut->m2[2][0] = (pAxis->x * pAxis->z) * (1.0f - fcos) + (pAxis->y * fsin);

	pOut->m2[0][1] = (pAxis->y * pAxis->x) * (1.0f - fcos) + (pAxis->z * fsin);
	pOut->m2[1][1] = (pAxis->y * pAxis->y) * (1.0f - fcos) + fcos;
	pOut->m2[2][1] = (pAxis->y * pAxis->z) * (1.0f - fcos) - (pAxis->x * fsin);


	pOut->m2[0][2] = (pAxis->z * pAxis->x) * (1.0f - fcos) - (pAxis->y * fsin);
	pOut->m2[1][2] = (pAxis->z * pAxis->y) * (1.0f - fcos) + (pAxis->x * fsin);
	pOut->m2[2][2] = (pAxis->z * pAxis->z) * (1.0f - fcos) + fcos;

	pOut->m2[0][3] = pOut->m2[1][3] = pOut->m2[2][3] = 0.f;
	pOut->m2[3][0] = pOut->m2[3][1] = pOut->m2[3][2] = 0.f;
	pOut->m2[3][3] = 1.f;
}
void FLOAT3x3XRotation(ssFLOAT3x3X *pOut, ssfloat angle, ssFLOAT3X *pAxis)
{
	pAxis->Normalize();

	ssfloat fcos = Cos(angle);
	ssfloat fsin = Sin(angle);

	pOut->m2[0][0] = (pAxis->x * pAxis->x) * (1.0f - fcos) + fcos;
	pOut->m2[1][0] = (pAxis->x * pAxis->y) * (1.0f - fcos) - (pAxis->z * fsin);
	pOut->m2[2][0] = (pAxis->x * pAxis->z) * (1.0f - fcos) + (pAxis->y * fsin);

	pOut->m2[0][1] = (pAxis->y * pAxis->x) * (1.0f - fcos) + (pAxis->z * fsin);
	pOut->m2[1][1] = (pAxis->y * pAxis->y) * (1.0f - fcos) + fcos;
	pOut->m2[2][1] = (pAxis->y * pAxis->z) * (1.0f - fcos) - (pAxis->x * fsin);


	pOut->m2[0][2] = (pAxis->z * pAxis->x) * (1.0f - fcos) - (pAxis->y * fsin);
	pOut->m2[1][2] = (pAxis->z * pAxis->y) * (1.0f - fcos) + (pAxis->x * fsin);
	pOut->m2[2][2] = (pAxis->z * pAxis->z) * (1.0f - fcos) + fcos;
}
void FLOAT4x4XRotationForColumnMatrix(ssFLOAT4x4X *pOut, ssfloat angle, ssFLOAT3X *pAxis)
{
	pAxis->Normalize();

	ssfloat fcos = Cos(angle);
	ssfloat fsin = Sin(angle);

	pOut->m2[0][0] = (pAxis->x * pAxis->x) * (1.0f - fcos) + fcos;
	pOut->m2[0][1] = (pAxis->x * pAxis->y) * (1.0f - fcos) - (pAxis->z * fsin);
	pOut->m2[0][2] = (pAxis->x * pAxis->z) * (1.0f - fcos) + (pAxis->y * fsin);

	pOut->m2[1][0] = (pAxis->y * pAxis->x) * (1.0f - fcos) + (pAxis->z * fsin);
	pOut->m2[1][1] = (pAxis->y * pAxis->y) * (1.0f - fcos) + fcos;
	pOut->m2[1][2] = (pAxis->y * pAxis->z) * (1.0f - fcos) - (pAxis->x * fsin);


	pOut->m2[2][0] = (pAxis->z * pAxis->x) * (1.0f - fcos) - (pAxis->y * fsin);
	pOut->m2[2][1] = (pAxis->z * pAxis->y) * (1.0f - fcos) + (pAxis->x * fsin);
	pOut->m2[2][2] = (pAxis->z * pAxis->z) * (1.0f - fcos) + fcos;

	pOut->m2[0][3] = pOut->m2[1][3] = pOut->m2[2][3] = 0.f;
	pOut->m2[3][0] = pOut->m2[3][1] = pOut->m2[3][2] = 0.f;
	pOut->m2[3][3] = 1.f;
}

void FLOAT4x4XScaling(ssFLOAT4x4X *pOut, ssfloat xAxis, ssfloat yAxis, ssfloat zAxis)
{
	pOut->Identity();
	pOut->m2[0][0] = xAxis;
	pOut->m2[1][1] = yAxis;
	pOut->m2[2][2] = zAxis;
}

void FLOAT3x3XScaling(ssFLOAT3x3X *pOut, ssfloat xAxis, ssfloat yAxis, ssfloat zAxis)
{
	pOut->Identity();
	pOut->m2[0][0] = xAxis;
	pOut->m2[1][1] = yAxis;
	pOut->m2[2][2] = zAxis;
}

//判断点在线段内
bool PointIsOnLine(const ssFLOAT3X *pa, const ssFLOAT3X *pb, const ssFLOAT3X *po)
{
	ssfloat ab = TwoPositionDistance3(pa->x, pa->y, pa->z, pb->x, pb->y, pb->z);
	ssfloat ao = TwoPositionDistance3(pa->x, pa->y, pa->z, po->x, po->y, po->z);
	ssfloat bo = TwoPositionDistance3(pb->x, pb->y, pb->z, po->x, po->y, po->z);
	return ZERO_E6(ab - ao - bo);
}
//判断点在三角形内
bool PointIsOnTriangle(const ssFLOAT3X *pa, const ssFLOAT3X *pb, const ssFLOAT3X *pc, const ssFLOAT3X *po)
{
	//return zero(vlen(xmult(subt(s1,s2),subt(s1,s3)))-vlen(xmult(subt(p,s1),subt(p,s2)))-vlen(xmult(subt(p,s2),subt(p,s3)))-vlen(xmult(subt(p,s3),subt(p,s1))));
	return ZERO_E4((*pa - *pb).Cross(*pa - *pc).Length() -
		(*po - *pa).Cross(*po - *pb).Length() -
		(*po - *pb).Cross(*po - *pc).Length() -
		(*po - *pc).Cross(*po - *pa).Length());
}
//求面法线
void GetFaceNormal(ssFLOAT3X *pOut, const ssFLOAT3X *pa, const ssFLOAT3X *pb, const ssFLOAT3X *pc)
{
	*pOut = (*pa - *pb).Cross(*pb - *pc);
}
//判断点在4面体内
bool PointIsInTetrahedron(const ssFLOAT3X *pa, const ssFLOAT3X *pb, const ssFLOAT3X *pc, const ssFLOAT3X *pd, const ssFLOAT3X *po)
{
	ssFLOAT4x3X mat0(pa->x, pa->y, pa->z,
		pb->x, pb->y, pb->z,
		pc->x, pc->y, pc->z,
		pd->x, pd->y, pd->z);
	ssFLOAT4x3X mat1(po->x, po->y, po->z,
		pb->x, pb->y, pb->z,
		pc->x, pc->y, pc->z,
		pd->x, pd->y, pd->z);
	ssFLOAT4x3X mat2(pa->x, pa->y, pa->z,
		po->x, po->y, po->z,
		pc->x, pc->y, pc->z,
		pd->x, pd->y, pd->z);
	ssFLOAT4x3X mat3(pa->x, pa->y, pa->z,
		pb->x, pb->y, pb->z,
		po->x, po->y, po->z,
		pd->x, pd->y, pd->z);
	ssFLOAT4x3X mat4(pa->x, pa->y, pa->z,
		pb->x, pb->y, pb->z,
		pc->x, pc->y, pc->z,
		po->x, po->y, po->z);
	ssfloat det[5];
	det[0] = mat0.Determinant4x4();
	if (ZERO_E6(det[0]))
		return true;
	det[1] = mat1.Determinant4x4();
	if (ZERO_E6(det[1]))
		return true;
	det[2] = mat2.Determinant4x4();
	if (ZERO_E6(det[2]))
		return true;
	det[3] = mat3.Determinant4x4();
	if (ZERO_E6(det[3]))
		return true;
	det[4] = mat4.Determinant4x4();
	if (ZERO_E6(det[4]))
		return true;

	return (det[0]<0.f&&det[1]<0.f&&det[2]<0.f&&det[3]<0.f&&det[4]<0.f) ||
		(det[0]>0.f&&det[1]>0.f&&det[2]>0.f&&det[3]>0.f&&det[4]>0.f);
}
//透视投影
ssFLOAT4x4X PerspectiveRH(const ssfloat width, const ssfloat hight, const ssfloat zNear, const ssfloat zFar)
{
	ssFLOAT4x4X mat;
	mat.Identity();
	mat._11 = 2.0f*zNear / width;
	mat._22 = 2.0f*zNear / hight;
	mat._33 = zFar / (zNear - zFar);
	mat._34 = -1.0f;
	mat._43 = zFar*zNear / (zNear - zFar);
	mat._44 = 0.0f;
	return mat;
}
ssFLOAT4x4X PerspectiveOffCenterRH(const ssfloat left, const ssfloat right, const ssfloat bottom, const ssfloat top, const ssfloat zNear, const ssfloat zFar)
{
	ssFLOAT4x4X mat;
	mat.Identity();
	mat._11 = 2.0f*zNear / (right - left);
	mat._22 = 2.0f*zNear / (top - bottom);
	mat._31 = (right + left) / (right - left);
	mat._32 = (top + bottom) / (top - bottom);
	mat._33 = -(zFar + zNear) / (zFar - zNear);
	mat._34 = -1.0f;
	mat._43 = -(2.0f * zFar * zNear) / (zFar - zNear);
	mat._44 = 0.0f;
	return mat;
}
ssFLOAT4x4X PerspectiveFovRH(const ssfloat fovy, const ssfloat aspect, const ssfloat zNear, const ssfloat zFar)
{
	ssfloat xmin, xmax, ymin, ymax;

	//ymax = zNear * tan(fovy * M_PI / 360.0);
	ymax = zNear * tanf(Degrees2Radian(fovy / 2));
	ymin = -ymax;
	xmin = ymin * aspect;
	xmax = ymax * aspect;
	return PerspectiveOffCenterRH(xmin, xmax, ymin, ymax, zNear, zFar);
}
ssFLOAT4x4X PerspectiveLH(const ssfloat width, const ssfloat hight, const ssfloat zNear, const ssfloat zFar)
{
	ssFLOAT4x4X mat;
	mat.Identity();
	mat._11 = 2.0f*zNear / width;
	mat._22 = 2.0f*zNear / hight;
	mat._33 = zFar / (zFar - zNear);
	mat._34 = 1.0f;
	mat._43 = zFar * zNear / (zNear - zFar);
	mat._44 = 0.0f;
	return mat;
}
ssFLOAT4x4X PerspectiveOffCenterLH(const ssfloat left, const ssfloat right, const ssfloat bottom, const ssfloat top, const ssfloat zNear, const ssfloat zFar)
{
	ssFLOAT4x4X mat;
	mat.Identity();
	mat._11 = 2.0f*zNear / (right - left);
	mat._22 = 2.0f*zNear / (top - bottom);
	mat._31 = (right + left) / (left - right);
	mat._32 = (top + bottom) / (bottom - top);
	mat._33 = zFar / (zFar - zNear);
	mat._34 = 1.0f;
	mat._43 = zNear * zFar / (zNear - zFar);
	mat._44 = 0.0f;
	return mat;
}
//正交投影
ssFLOAT4x4X OrthoRH(const ssfloat width, const ssfloat hight, const ssfloat zNear, const ssfloat zFar)
{
	ssFLOAT4x4X mat;
	mat.Identity();
	mat._11 = 2.0f / width;
	mat._22 = 2.0f / hight;
	mat._33 = 1.0f / (zNear - zFar);
	mat._43 = zNear / (zNear - zFar);
	return mat;
}
ssFLOAT4x4X OrthoOffCenterRH(const ssfloat left, const ssfloat right, const ssfloat bottom, const ssfloat top, const ssfloat zNear, const ssfloat zFar)
{
	ssFLOAT4x4X mat;
	mat.Identity();
	mat._11 = 2.0f / (right - left);
	mat._22 = 2.0f / (top - bottom);
	mat._33 = 1.0f / (zNear - zFar);
	mat._41 = (left + right) / (left - right);
	mat._42 = (top + bottom) / (bottom - top);
	mat._43 = zNear / (zNear - zFar);
	return mat;
}
ssFLOAT4x4X OrthoLH(const ssfloat width, const ssfloat hight, const ssfloat zNear, const ssfloat zFar)
{
	ssFLOAT4x4X mat;
	mat.Identity();
	mat._11 = 2.0f / width;
	mat._22 = 2.0f / hight;
	mat._33 = 1.0f / (zFar - zNear);
	mat._43 = -zNear / (zFar - zNear);
	return mat;
}
ssFLOAT4x4X OrthoOffCenterLH(const ssfloat left, const ssfloat right, const ssfloat bottom, const ssfloat top, const ssfloat zNear, const ssfloat zFar)
{
	ssFLOAT4x4X mat;
	mat.Identity();
	mat._11 = 2.0f / (right - left);
	mat._22 = 2.0f / (top - bottom);
	mat._33 = 1.0f / (zFar - zNear);
	mat._41 = (right + left) / (left - right);
	mat._42 = (top + bottom) / (bottom - top);
	mat._43 = zNear / (zNear - zFar);
	return mat;
}
//浮点数插值
ssfloat FloatLerp(ssfloat a, ssfloat b, ssfloat per)
{
	if (per > 1.0f) per = 1.0f;
	ssfloat diff = b - a;
	return a + diff * per;
}


void VectorLerp(ssFLOAT3X *pOut, ssFLOAT3X *pIn1, ssFLOAT3X *pIn2, ssfloat per)
{
	if (per > 1.0f) per = 1.0f;
	ssFLOAT3X diff = *pIn2 - *pIn1;
	diff *= per;
	*pOut = *pIn1 + diff;
}

void QuaternionToMatrix3x3(ssFLOAT4X *pQ, ssFLOAT3x3X *pM)
{
	pM->_11 = 1.0f - 2.0f*pQ->y*pQ->y - 2.0f*pQ->z*pQ->z;
	pM->_12 = 2.0f*pQ->x*pQ->y + 2.0f*pQ->z*pQ->w;
	pM->_13 = 2.0f*pQ->x*pQ->z - 2.0f*pQ->y*pQ->w;
	pM->_21 = 2.0f*pQ->x*pQ->y - 2.0f*pQ->z*pQ->w;
	pM->_22 = 1.0f - 2.0f*pQ->x*pQ->x - 2.0f*pQ->z*pQ->z;
	pM->_23 = 2.0f*pQ->y*pQ->z + 2.0f*pQ->x*pQ->w;
	pM->_31 = 2.0f*pQ->x*pQ->z + 2.0f*pQ->y*pQ->w;
	pM->_32 = 2.0f*pQ->y*pQ->z - 2.0f*pQ->x*pQ->w;
	pM->_33 = 1.0f - 2.0f*pQ->x*pQ->x - 2.0f*pQ->y*pQ->y;
}

void QuaternionToMatrix4x4(ssFLOAT4X *pQ, ssFLOAT4x4X *pM)
{
	pM->Identity();
	pM->_11 = 1.0f - 2.0f*pQ->y*pQ->y - 2.0f*pQ->z*pQ->z;
	pM->_12 = 2.0f*pQ->x*pQ->y + 2.0f*pQ->z*pQ->w;
	pM->_13 = 2.0f*pQ->x*pQ->z - 2.0f*pQ->y*pQ->w;
	pM->_21 = 2.0f*pQ->x*pQ->y - 2.0f*pQ->z*pQ->w;
	pM->_22 = 1.0f - 2.0f*pQ->x*pQ->x - 2.0f*pQ->z*pQ->z;
	pM->_23 = 2.0f*pQ->y*pQ->z + 2.0f*pQ->x*pQ->w;
	pM->_31 = 2.0f*pQ->x*pQ->z + 2.0f*pQ->y*pQ->w;
	pM->_32 = 2.0f*pQ->y*pQ->z - 2.0f*pQ->x*pQ->w;
	pM->_33 = 1.0f - 2.0f*pQ->x*pQ->x - 2.0f*pQ->y*pQ->y;
}

void Matrix3x3ToQuaternion(ssFLOAT3x3X *pM, ssFLOAT4X *pQ)
{
	pQ->w = 0.5f * Math3D::Sqrt(1.0f+pM->_11+pM->_22+pM->_33);
	pQ->x = (pM->_23 - pM->_32) / (4.0f*pQ->w);
	pQ->y = (pM->_31 - pM->_13) / (4.0f*pQ->w);
	pQ->z = (pM->_12 - pM->_21) / (4.0f*pQ->w);
}

void FLOAT4XTransformNormal(ssFLOAT3X *pOut, const ssFLOAT4X *pQ, ssFLOAT3X *pV)
{
	ssFLOAT3X uv, uuv, qv(pQ->x, pQ->y, pQ->z);
	uv = qv.Cross(*pV);
	uuv = qv.Cross(uv);
	uv *= (2.0f*pQ->w);
	uuv *= 2.0f;
	*pOut = *pV + uv + uuv;
}

bool RayIntersectsPlane(ssFLOAT3X& n, ssFLOAT3X& p1, ssFLOAT3X& p0, ssFLOAT3X& d, ssFLOAT3X* intersection)
{
	ssfloat t = (n.Dot(p1) - n.Dot(p0)) / (n.Dot(d));
	if (t >= 0.0f && intersection)
	{
		*intersection = p0 + (d * t);
	}
	return t >= 0.0f;
}

bool ScreenToWorld(const ssFLOAT3X *pInCoord, const ssFLOAT4x4X *pInView, const ssFLOAT4x4X *pInProjection, const ssViewPort *pInViewPort, ssFLOAT3X *pOut)
{
	ssMatrix4x4 viewProj = (*pInView)*(*pInProjection);
	if (viewProj.Invert()==false)
	{
		return false;
	}
	ssVector4 v1((pInCoord->x - (float)pInViewPort->m[0]) / (float)pInViewPort->m[2] * 2.0f - 1.0f, 
				(pInCoord->y - (float)pInViewPort->m[1]) / (float)pInViewPort->m[3] * 2.0f - 1.0f,
				2.0f*pInCoord->z - 1.0f,
				1.0f);
	ssVector4 v2;
	FLOAT4x4XTransformVector4(&v2, &viewProj, &v1);
	if (v2.m[3] == 0.0f)
		return false;
	v2.m[3] = 1.0f / v2.m[3];

	pOut->m[0] = v2.m[0] * v2.m[3];
	pOut->m[1] = v2.m[1] * v2.m[3];
	pOut->m[2] = v2.m[2] * v2.m[3];
	return true;
}

bool WorldToScreen(const ssFLOAT3X *pInCoord, const ssFLOAT4x4X *pInView, const ssFLOAT4x4X *pInProjection, const ssViewPort *pInViewPort, ssFLOAT3X *pOut)
{
	ssfloat temp[8];

	temp[0] = pInView->m[0] * pInCoord->x + pInView->m[4] * pInCoord->y + pInView->m[8] * pInCoord->z + pInView->m[12];
	temp[1] = pInView->m[1] * pInCoord->x + pInView->m[5] * pInCoord->y + pInView->m[9] * pInCoord->z + pInView->m[13];
	temp[2] = pInView->m[2] * pInCoord->x + pInView->m[6] * pInCoord->y + pInView->m[10] * pInCoord->z + pInView->m[14];
	temp[3] = pInView->m[3] * pInCoord->x + pInView->m[7] * pInCoord->y + pInView->m[11] * pInCoord->z + pInView->m[15];

	temp[4] = pInProjection->m[0] * temp[0] + pInProjection->m[4] * temp[1] + pInProjection->m[8] * temp[2] + pInProjection->m[12] * temp[3];
	temp[5] = pInProjection->m[1] * temp[0] + pInProjection->m[5] * temp[1] + pInProjection->m[9] * temp[2] + pInProjection->m[13] * temp[3];
	temp[6] = pInProjection->m[2] * temp[0] + pInProjection->m[6] * temp[1] + pInProjection->m[10] * temp[2] + pInProjection->m[14] * temp[3];
	temp[7] = -temp[2];

	if (temp[7] == 0.0f)
		return false;
	temp[7] = 1.0f / temp[7];

	temp[4] *= temp[7];
	temp[5] *= temp[7];
	temp[6] *= temp[7];

	pOut->m[0] = (temp[4] * 0.5f + 0.5f)*pInViewPort->m[2] + pInViewPort->m[0];
	pOut->m[1] = (temp[5] * 0.5f + 0.5f)*pInViewPort->m[3] + pInViewPort->m[1];
	pOut->m[2] = (1.0f + temp[6])*0.5f;
	return true;
}

void DecomposingVectorIntoCoordinateSystems(const ssFLOAT3X & in, const ssFLOAT3X & axis1, const ssFLOAT3X & axis2, const ssFLOAT3X & axis3, ssFLOAT3X & out1, ssFLOAT3X & out2, ssFLOAT3X & out3)
{
	out1 = axis1 * in.Dot(axis1);
	out2 = axis2 * in.Dot(axis2);
	out3 = axis3 * in.Dot(axis3);
}

StarSeeker::ssfloat Point2LineDistance(const ssFLOAT3X& m, const ssFLOAT3X& begin, const ssFLOAT3X& dir)
{
	//点为M，线段起点为A，终点为B，AM为向量a，AB为向量b，向量c为向量a在向量b上的投影，向量e为M点到AB的垂线，求出向量e的模就是点到线段的距离
	ssFLOAT3X a = m - begin;
	const ssFLOAT3X& b = dir;
	ssFLOAT3X c = b * a.Dot(b) / b.SquaredLength();//(b * (a.Dot(b)) / b.SquaredLength()
	ssFLOAT3X e = a - c;
	return e.SquaredLength();
}

StarSeeker::ssfloat Point2LineDistance2D(const ssFLOAT2X& m, const ssFLOAT2X& begin, const ssFLOAT2X& dir)
{
	ssFLOAT2X a = m - begin;
	const ssFLOAT2X& b = dir;
	ssFLOAT2X c = b * a.Dot(b) / b.SquaredLength();
	ssFLOAT2X e = a - c;
	return e.SquaredLength();
}

StarSeeker::ssFLOAT3X Point2LineProjection(const ssFLOAT3X& m, const ssFLOAT3X& begin, const ssFLOAT3X& dir)
{
	ssFLOAT3X a = m - begin;
	const ssFLOAT3X& b = dir;
	ssFLOAT3X c = b * a.Dot(b) / b.SquaredLength();
	return begin + c;
}

StarSeeker::ssFLOAT2X Point2LineProjection2D(const ssFLOAT2X& m, const ssFLOAT2X& begin, const ssFLOAT2X& dir)
{
	ssFLOAT2X a = m - begin;
	const ssFLOAT2X& b = dir;
	ssFLOAT2X c = b * a.Dot(b) / b.SquaredLength();
	return begin + c;
}

StarSeeker::ssFLOAT2X Rotate2dVector(const ssFLOAT2X& v, ssfloat radian)
{
	//x1 =  x*cosθ- y * sinθ     y1 = y*cosθ + x * sinθ
	return ssFLOAT2X(v.x*Cos(radian)-v.y*Sin(radian), v.y*Cos(radian)+v.x*Sin(radian));
}

StarSeeker::ssFLOAT2X CoordRotation2D(const ssFLOAT2X& p, const ssFLOAT2X& dir)
{
	ssFLOAT2X ret;
	static ssVector2 dir0(0, 1);
	ssfloat sita = Float2XIncludedAngle(&dir0, &dir);
	if (dir.x < 0)
		sita = Math3D::gc_DOUBLE_PI - sita;

	ret.x = p.x * Math3D::Cos(sita) + p.y * Math3D::Sin(sita);
	ret.y = p.y * Math3D::Cos(sita) - p.x * Math3D::Sin(sita);
	return ret;
}

bool LineIntersectsCircle(const ssFLOAT2X& center, ssfloat r, const ssFLOAT2X& lineBegin, const ssFLOAT2X& lineDir)
{
	ssfloat r2 = r * r;
	ssFLOAT2X lineEnd = lineBegin + lineDir;
	//是否有点在圆内
	ssFLOAT2X a = center - lineBegin;
	if (a.SquaredLength() <= r2)
		return true;
	ssFLOAT2X d = center - lineEnd;
	if (d.SquaredLength() <= r2)
		return true;
	//2个点都不在圆内，计算圆心在线段上的投影
	const ssFLOAT2X& b = lineDir;
	ssFLOAT2X c = b * a.Dot(b) / b.SquaredLength();
	ssFLOAT2X p = lineBegin + c;
	//投影点不在线段中间则不相交
	f32 len = lineDir.SquaredLength();
	if ((p - lineBegin).SquaredLength() > len || (p - lineEnd).SquaredLength() > len)
		return false;
	//投影点在线段中间,判断圆心到线段的距离是否小于半径
	ssFLOAT2X e = a - c;

	return e.SquaredLength() <= r2;
}

bool PointInTheMiddleOfTheLine(const ssFLOAT3X& lineBegin, const ssFLOAT3X& lineEnd, const ssFLOAT3X& p)
{
	f32 len = (lineEnd - lineBegin).SquaredLength();
	if ((p - lineBegin).SquaredLength() > len || (p - lineEnd).SquaredLength() > len)
		return false;
	else
		return true;
}

bool PointInTheMiddleOfTheLine2D(const ssFLOAT2X& lineBegin, const ssFLOAT2X& lineEnd, const ssFLOAT2X& p)
{
	f32 len = (lineEnd - lineBegin).SquaredLength();
	if ((p - lineBegin).SquaredLength() > len || (p - lineEnd).SquaredLength() > len)
		return false;
	else
		return true;
}

}
}
